#include "app_ctrl.h"

/**
 * @brief:    App_Init_Enable
 * @details:  初始化
 */
motor_gm6020_t motor_1;
motor_dm_t motor_2;
motor_dm_t motor_3;
uint8_t App_Init_Enable(void)
{
  /* 算法、驱动初始化 */
  BSP_CAN_Init();
  NRF24L01_Init();
  
  // App_GravityPID_Init();
  // App_SetGravityTargetVel(MOTOR_TX_ID_2, 0.0f);   // 初始化目标速度为0（抵抗重力）
  // App_SetGravityTargetVel(MOTOR_TX_ID_3, 0.0f);
  
  // Algo_PID_Init(&pid_spd_to_vol, 30, 0, 0, 30000, 30000);
  // Algo_PID_Init(&pid_ang_to_spd, 0.08, 0.002, 0.02, 50, 100);
  // DJI_Channel_Init();
  
  /* 电机初始化 */
  GM6020_Init(&motor_1, MOTOR_RX_ID_1);

  DM_Init(&motor_2, MOTOR_TX_ID_2, MOTOR_RX_ID_2, mit_mode);
  // DM_Init(&motor_2, 0x04, 0x14, mit_mode); // 临时调试

  DM_Init(&motor_3, MOTOR_TX_ID_3, MOTOR_RX_ID_3, mit_mode);
  
  BSP_TIM_EnableElapsedCallback();

  /* 回零点 */
  GM6020_GotoZero();
  // DM_GotoZero(&motor_2);
  // DM_GotoZero(&motor_3);
  DM_ResetZero(&motor_2);
  DM_ResetZero(&motor_3);

  /* 电机使能 */
  HAL_Delay(30);
  DM_Enable(&motor_2);
  HAL_Delay(30);
  DM_Enable(&motor_3);
  HAL_Delay(30);
  
  return 1;
}


/**
 * @brief:    App_MainLoop
 * @details:  定时中断的处理函数，放在主函数的while(1)中
 */
uint8_t App_MainLoop(void)
{
  // 定时中断处理的各个任务
  // 任务1
  
  App_UpdateSendData();
  uint8_t flag = App_NormalizeAndSendData();
  App_GetFloatAngle();
  
  // 任务2 重力补偿
  // App_UpdateGravityData();
  // App_Gravity_Compensation(2);
  // App_Gravity_Compensation(3);
  
  // 任务，复位
  // uint16_t out = GM6020_CalcOutput(&motor_1, 2500);
  // DJI_RegData(3, motor_1.in_id, out);
  // flag &= DJI_SendData(&dji_channel_3);
  
  return flag;
}

/**
 * @brief:    App_UpdateSendData
 * @param:    void
 * @retval:   uint8_t 状态
 * @details:  通过CAN反馈，更新app_raw_data数据包，用于发送机械角度数据
 */
uint32_t app_raw_data[6] = {0};
uint8_t App_UpdateSendData(void)
{
  DM_RequestFB(&motor_2);
  DM_RequestFB(&motor_3);
  HAL_Delay(1);

  /* 电机1：GM6020，反馈机械角度  
     DATA[0] 机械角度高8位；DATA[1] 机械角度低8位 */ 
  app_raw_data[0] = 0x0000FFFF & ((((uint16_t)can_rx_data[0][0]) << 8) | can_rx_data[0][1]);

  /* 电机2：DM J4310，反馈机械角度  
     DATA[1] 机械角度高8位；DATA[2] 机械角度低8位 */ 
  app_raw_data[1] = 0x0000FFFF & ((((uint16_t)can_rx_data[1][1]) << 8) | can_rx_data[1][2]);

  /* 电机3：DM J4310，反馈机械角度  
     DATA[1] 机械角度高8位；DATA[2] 机械角度低8位 */ 
  app_raw_data[2] = 0x0000FFFF & ((((uint16_t)can_rx_data[2][1]) << 8) | can_rx_data[2][2]);

  /* 编码器4：OidEncoder，反馈机械角度  
     Data[3] [7:0], Data[4] [15:8], Data[5] [23:16], Data[6]:[31:24] */ 
  app_raw_data[3] = 0xFFFFFFFF & ((((uint32_t)can_rx_data[3][3]) << 0)  | 
                                  (((uint32_t)can_rx_data[3][4]) << 8)  |
                                  (((uint32_t)can_rx_data[3][5]) << 16) |
                                  (((uint32_t)can_rx_data[3][6]) << 24));
  
  /* 电机5：M3508，反馈机械角度  
     DATA[0] 机械角度高8位；DATA[1] 机械角度低8位 */ 
  app_raw_data[4] = 0x0000FFFF & ((((uint16_t)can_rx_data[4][0]) << 8) | can_rx_data[4][1]);

  /* 电机6：M3508，反馈机械角度  
     DATA[0] 机械角度高8位；DATA[1] 机械角度低8位 */ 
  app_raw_data[5] = 0x0000FFFF & ((((uint16_t)can_rx_data[5][0]) << 8) | can_rx_data[5][1]);

  return 1;
}

/**
 * @brief:    App_SendData
 * @details:  通过无线收发模块将app_send_data发送出去
 */
uint16_t app_send_data[6] = {0};
uint8_t App_NormalizeAndSendData(void)
{
  // 准备数据
  app_send_data[0] = (uint16_t)app_raw_data[0];
  app_send_data[1] = (uint16_t)app_raw_data[1];
  app_send_data[2] = (uint16_t)app_raw_data[2];
  app_send_data[3] = (uint16_t)app_raw_data[3];
  app_send_data[4] = (uint16_t)app_raw_data[4];
  app_send_data[5] = (uint16_t)app_raw_data[5];

  // 发送数据
  return NRF24L01_UpdateAndSend(app_send_data);;
}

/**
 * @brief:    测试数据的范围是否正确
 */
float app_watch_angles[6] = {0};
unsigned int app_watch_int[6] = {0};
uint8_t App_GetFloatAngle(void) 
{ 
  app_watch_angles[0] = ((float)(app_send_data[0] - ((1<<13)/2))) / ((float)(1<<13)) * 6.28f;
  
  app_watch_angles[1] = uint_to_float(app_send_data[1], -12.5, 12.5, 16);
  app_watch_angles[2] = uint_to_float(app_send_data[2], -12.5, 12.5, 16);
  
  app_watch_angles[3] = ((float)(app_send_data[3] - ((1<<15)/2))) / ((float)(1<<15)) * 6.28f;

  app_watch_int[0] = app_send_data[0];
  app_watch_int[1] = app_send_data[1];
  app_watch_int[2] = app_send_data[2];
  app_watch_int[3] = app_send_data[3];
  


  // app_send_angles[4] = ((float)(app_send_data[4] - ((1<<13)/2))) / ((float)(1<<13)) * 6.28f;
  // app_send_angles[5] = ((float)(app_send_data[5] - ((1<<13)/2))) / ((float)(1<<13)) * 6.28f;
  
  return 1;
}

/**
 * @brief:    App_UpdateGravityData
 * @details:  获取两个电机的当前转速，用于重力补偿
 */
float app_gravity_vel_data[2] = {0};   // 两个速度值
float app_gravity_pos_data[2] = {0};   // 两个位置值
uint8_t App_UpdateGravityData(void)
{
	// 速度在D[3]和D[4]中，12位无符号，线性映射 [0,4095] -> [-VMAX, VMAX]
	uint16_t raw_vel_2 = (((int16_t)can_rx_data[1][3]) << 4) | (can_rx_data[1][4] >> 4);  
	uint16_t raw_vel_3 = (((int16_t)can_rx_data[2][3]) << 4) | (can_rx_data[2][4] >> 4);
  // 速度在D[1]和D[2]中，16位无符号，线性映射 [0,16383] -> [-VMAX, VMAX]
  uint16_t raw_pos_2 = (((int16_t)can_rx_data[1][1]) << 8 | can_rx_data[1][2]);
  uint16_t raw_pos_3 = (((int16_t)can_rx_data[2][1]) << 8 | can_rx_data[2][2]);

	// 转换为实际速度 (rad/s)，假设最大速度±100rad/s
	app_gravity_vel_data[0] = App_Uint2Float(raw_vel_2, DM_VEL_FLOAT_MIN, DM_VEL_FLOAT_MAX, 12);
	app_gravity_vel_data[1] = App_Uint2Float(raw_vel_3, DM_VEL_FLOAT_MIN, DM_VEL_FLOAT_MAX, 12);

  app_gravity_pos_data[0] = App_Uint2Float(raw_pos_2, DM_POS_FLOAT_MIN, DM_POS_FLOAT_MAX, 16);
  app_gravity_pos_data[1] = App_Uint2Float(raw_pos_3, DM_POS_FLOAT_MIN, DM_POS_FLOAT_MAX, 16);
  return 1;
}

/**
 * @brief:    重力补偿计算（使用速度环）
 * @details:  读取当前速度，计算重力补偿转矩，通过速度模式维持零速
 */
algo_pid_t gravity_vel_pid_2 = {0};  // 电机2速度环
algo_pid_t gravity_vel_pid_3 = {0};  // 电机3速度环
float target_vel_2 = 0.0f;
float target_vel_3 = 0.0f;
void App_GravityPID_Init(void)
{
  Algo_PID_Init(&gravity_vel_pid_2, 0.5f, 0.1f, 0.05f, 50.0f, 5.0f);
  Algo_PID_Init(&gravity_vel_pid_3, 0.5f, 0.1f, 0.05f, 50.0f, 5.0f);
}
uint8_t App_Gravity_Compensation(uint8_t tx_id)
{
  float comp_torque = 0.0f;
  float speed_threshold = 5.0f; // 速度阈值，单位rad/s
  uint8_t flag = 1;
  if (tx_id == 2) {
    if (app_gravity_vel_data[0] < -speed_threshold) { comp_torque = 0.5f; }     // 负速度施加小正扭矩 
    else if (app_gravity_vel_data[0] > speed_threshold) { comp_torque = -0.5f; }// 正速度施加小负扭矩
    flag &= DM_DriveMITCtrl(&hcan1, motor_2.id, 0.0f, 0.0f, 0.0f, 0.0f, comp_torque);
  } 
  else if (tx_id == 3) {
    if (app_gravity_vel_data[1] < -speed_threshold) { comp_torque = 0.5f; } 
    else if (app_gravity_vel_data[1] > speed_threshold) { comp_torque = -0.5f; }
    flag &= DM_DriveMITCtrl(&hcan1, motor_3.id, 0.0f, 0.0f, 0.0f, 0.0f, comp_torque);
  }
  return flag;
}
// 设置目标速度函数（通常设为0来抵抗重力）
void App_SetGravityTargetVel(uint8_t motor_id, float target_velocity)
{
  if (motor_id == MOTOR_TX_ID_2) {
    target_vel_2 = target_velocity;
    gravity_vel_pid_2.error_int = 0;  // 重置积分器
  } else if (motor_id == MOTOR_TX_ID_3) {
    target_vel_3 = target_velocity; 
    gravity_vel_pid_3.error_int = 0;
  }
}


int App_Float2Uint(float x_float, float x_min, float x_max, uint8_t bits)
{
  float span = x_max - x_min;
  float offset = x_min;
  return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
float App_Uint2Float(uint16_t x_int, float x_min, float x_max, uint8_t bits)
{
  float span = x_max - x_min;
  float offset = x_min;
  return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
